/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "RangingDisplay.hpp"
#include <GL/gl.h>
#include <GL/glut.h>
#include "PositionClient.hpp"

const int CRangingDisplay::backlogMutex=0;
const int CRangingDisplay::historyMutex=1;

CRangingDisplay::CRangingDisplay()
:CSync(0,2),
z(0),
displayDots(true),
displayClearance(false),
clearanceAlpha(0.5),
displayHistory(false),
historyLifetime_ms(4000),
referencePosition(NULL)
//poseBacklog(4000,10000,gim::KBackLogReferenceLastElement )
{
  
}

void CRangingDisplay::Draw()
{
  glMatrixMode(GL_MODELVIEW);
  
  glColor3f(1,0,0);
  
  float d, h;
  
  Lock(historyMutex);
  
  if (history.size())
  {
    gim::time current(true);
    std::list<RangingWPose>::iterator historyIter=history.begin();
    do
    {
      MaCI::Ranging::TDistanceArray::iterator distIter;
      
      float color=0.8;
      if (historyLifetime_ms)
      {
        int diff_ms=(historyIter->time-current).getTimeMs();
        color=diff_ms/historyLifetime_ms*0.8;
      }
      glColor3f(0.2+color,0,0);
      
      glPushMatrix();
      //glLoadIdentity();
      glTranslatef(historyIter->pose.x, 
                   historyIter->pose.y, 0);
      glRotatef((historyIter->pose.a)*(180.0/M_PI), 0, 0, 1);
      
      glTranslatef(devicePosition.x, 
                   devicePosition.y, 0);
      glRotatef((devicePosition.heading)*(180.0/M_PI), 0, 0, 1);
//       glTranslatef(historyIter->pose.x+devicePosition.x, 
//                    historyIter->pose.y+devicePosition.y, 0);
//       glRotatef((historyIter->pose.a+devicePosition.heading)*(180.0/M_PI), 0, 0, 1);
      
      glPointSize(2);
      glBegin(GL_POINTS);
      
      glVertex2f(0,0);
      
      for (distIter=historyIter->distances.begin(); distIter!=historyIter->distances.end(); distIter++)
      {
        d=distIter->distance;
        if (d >0) {
          h=distIter->angle;
          glVertex2f(cos(h)*d,sin(h)*d);
        }
      }
      
      glEnd();
      
      glPopMatrix();
      
      historyIter++;
    } while (displayHistory && historyIter!=history.end());

    
  }
  
  Unlock(historyMutex);
  
}

void CRangingDisplay::Draw(const float x, const float y, const float heading, const MaCI::Ranging::TDistanceArray *distances)
{
  MaCI::Ranging::TDistanceArray::const_iterator distIter;
  const MaCI::Ranging::TDistanceArray *da;
  float d, h;
  
  if (distances==NULL)
  {
    
    if (history.size())
    {
      Lock(historyMutex);
      da=&(history.begin()->distances);
    } else {
      return;
    }
  } else {
    da=distances;
  }
  glPushMatrix();
  glTranslatef(x, y, 0);
  glRotatef(heading*(180.0/M_PI), 0, 0, 1);


  glTranslatef(devicePosition.x, 
               devicePosition.y, 0);
  glRotatef((devicePosition.heading)*(180.0/M_PI), 0, 0, 1);
  glPointSize(2);
  glBegin(GL_POINTS);
      
  for (distIter=da->begin(); distIter!=da->end(); distIter++)
  {
    d=distIter->distance;
    if (d >0) {
      h=distIter->angle;
      glVertex2f(cos(h)*d,sin(h)*d);
    }
  }
      
  glEnd();
      
  glPopMatrix();
  
  if (distances==NULL)
    Unlock(historyMutex);
}

void CRangingDisplay::OnRangingDataEvent(const MaCI::Ranging::CRangingData& data,
                        unsigned int seq, MaCI::Ranging::CRangingClient& client)
{
  MaCI::Common::TTimestamp time;
  
  Lock(historyMutex);
  history.push_front(RangingWPose());
  
  if (client.GetDistanceArray(history.front().distances, 
                              &(history.front().header), &time, NULL, 0))
  {
    gim::time diff;
    /*Lock(backlogMutex);
    poseBacklog.PeekData(history.front().pose, time.GetGimTime(), &diff);
    Unlock(backlogMutex);*/
    MaCI::Position::CPositionData d;
    if (referencePosition && 
        referencePosition->GetPositionEvent( d, time.GetGimTime(), &diff) &&
        d.GetPose2D())
    //TODO: fix this
    {
      history.front().pose.x=d.GetPose2D()->x;
      history.front().pose.y=d.GetPose2D()->y;
      history.front().pose.a=d.GetPose2D()->a;
    } else {
      history.front().pose.x=0;
      history.front().pose.y=0;
      history.front().pose.a=0;
    }
    history.front().time.setToCurrentPlus(historyLifetime_ms);
    //dPrint(1,"diff=%d",diff.getTimeMs());;
  } else {
    history.pop_front();
  }
  
  gim::time current(true);
  while (history.size()>1 && history.back().time<current)
  {
    history.pop_back();
  }
  Unlock(historyMutex);
}
/*
void CRangingDisplay::SetPosition(const MaCI::Position::CPositionData &position)
{
  Lock(backlogMutex);
  poseBacklog.PushData(position.GetTimestamp()->GetGimTime(),*(position.GetPose2D()));
  Unlock(backlogMutex);
}*/

void CRangingDisplay::SetReferencePosition(MaCI::Position::CPositionClient *positionClient)
{
  referencePosition=positionClient;
}

void CRangingDisplay::SetDeviceInformation(const MaCI::Ranging::TDeviceInformationLimits limits,
                          const MaCI::Ranging::TDeviceInformationPosition devicePosition)
{
  this->limits=limits;
  this->devicePosition=devicePosition;
}

void CRangingDisplay::SetHistoryLengthMs(int historyLength_ms)
{
   historyLifetime_ms=historyLength_ms;
}

void CRangingDisplay::EnableHistory(bool history)
{
  displayHistory=history;
}

bool CRangingDisplay::IsHistoryEnabled()
{
  return displayHistory;
}

